#include "DJI/M3508.h"
#include <cmath>
#include <utility>
#include <filesystem>
#include <fstream>

namespace MM {
    constexpr float ECD_TO_ANGLE_DJI = 0.043945f;
    constexpr float RPM_2_ANGLE_PER_SEC = 6.0f;


    using std::tie;
    using std::move;
    using std::lock_guard, std::exception;
    using std::to_string, std::vformat, std::make_format_args;
    using std::make_unique;

    M3508RawStatus::operator can_frame() const {
        can_frame frame{};
        frame.can_id = canId;
        frame.len = 8;
        frame.data[0] = static_cast<uint8_t>(ecd >> 8);
        frame.data[1] = static_cast<uint8_t>(ecd & 0xFF);
        frame.data[2] = static_cast<uint8_t>(rpm >> 8);
        frame.data[3] = static_cast<uint8_t>(rpm & 0xFF);
        frame.data[4] = static_cast<uint8_t>(current >> 8);
        frame.data[5] = static_cast<uint8_t>(current & 0xFF);
        frame.data[6] = temperature;
        return frame;
    }

    M3508RawStatus::M3508RawStatus(const can_frame &frame) {
        canId = frame.can_id;
        ecd = static_cast<uint16_t>(frame.data[0] << 8) | frame.data[1];
        rpm = static_cast<int16_t>(static_cast<uint16_t>(frame.data[2] << 8) | frame.data[3]);
        current = static_cast<int16_t>(static_cast<uint16_t>(frame.data[4] << 8) | frame.data[5]);
        temperature = frame.data[6];
    }

    string M3508RawStatus::format() const {
        return vformat("ID: {:X}, Ang: {} RPM, ecd: {} / 8191, CRT: {} mA, temperature: {} deg",
                       make_format_args(canId, rpm, ecd, current, temperature));
    }


    M3508RawControl::operator can_frame() const {
        can_frame frame{};
        frame.can_id = canId;
        frame.len = 8;
        frame.data[0] = static_cast<uint8_t>(ampere[0] >> 8);
        frame.data[1] = static_cast<uint8_t>(ampere[0] & 0xFF);
        frame.data[2] = static_cast<uint8_t>(ampere[1] >> 8);
        frame.data[3] = static_cast<uint8_t>(ampere[1] & 0xFF);
        frame.data[4] = static_cast<uint8_t>(ampere[2] >> 8);
        frame.data[5] = static_cast<uint8_t>(ampere[2] & 0xFF);
        frame.data[6] = static_cast<uint8_t>(ampere[3] >> 8);
        frame.data[7] = static_cast<uint8_t>(ampere[3] & 0xFF);
        return frame;
    }

    M3508RawControl::M3508RawControl(const can_frame &frame): canId(frame.can_id) {
        ampere[0] = static_cast<int16_t>(static_cast<uint16_t>(frame.data[0] << 8 | frame.data[1]));
        ampere[1] = static_cast<int16_t>(static_cast<uint16_t>(frame.data[2] << 8 | frame.data[3]));
        ampere[2] = static_cast<int16_t>(static_cast<uint16_t>(frame.data[4] << 8 | frame.data[5]));
        ampere[3] = static_cast<int16_t>(static_cast<uint16_t>(frame.data[6] << 8 | frame.data[7]));
    }

    M3508RawControl::M3508RawControl(const array<int16_t, 4> &&ampere, const uint16_t canId): canId(canId) {
        std::ranges::move(ampere, this->ampere.begin());
    }

    M3508s::~M3508s() {
        logger.force_flush();
    }

    M3508s::M3508s(string canInterface, optional<string> &&loggerId, const bool loggingToTextfile,
                   const bool loggingToConsole): interface(std::move(canInterface)),
                                                 id(loggerId.value_or(to_string(random_number(0, 2000)))),
                                                 logger(
                                                     [&] {
                                                         const string filename = "M3508_" + loggerId.value_or(
                                                                 to_string(random_number(0, 2000)));
                                                         const string filepath =
                                                                 string(MM_LOG_DIR) + "/M3508/" + filename;
                                                         return log::create_log(filepath,
                                                             configGenerate(
                                                                 std::move(filepath), loggingToConsole,
                                                                 loggingToTextfile)
                                                         );
                                                     }()) {
        can_driver = make_unique<canpp::CanDriver<
        > >((canpp::CanDriverConfig{
            interface, id, std::set<uint16_t>{0x201, 0x202, 0x203, 0x204, 0x205, 0x206, 0x207, 0x208}
        }));
        PIDInstance pos_pid{5, 0, 0, 400},
                vel_pid{5, 0.00001, 0, 10000, 5000},
                cur_pid{2, 0.0001, 0.0005, 10000, 5000};
        std::ranges::fill(pos_pids, pos_pid);
        std::ranges::fill(ang_pids, vel_pid);
        std::ranges::fill(cur_pids, cur_pid);

        (void) logger.info("M3508s successfully inited!");
    }

    inline M3508RawStatus M3508s::transformStatus(const can_frame &frame) const {
        const auto msg = static_cast<M3508RawStatus>(frame);
        (void) logger.info("Receiving Status:\n {}", msg.format());
        fuse_check(msg);
        return msg;
    }

    template<bool Plotting>
    void M3508s::enable() {
        can_driver->registerReadingCallback([&](can_frame &&frame) {
            TimePoint pt = steady_clock::now();

            auto msg = transformStatus(frame);

            const auto index = (msg.canId ^ 0x200) - 1;
            uint16_t senderCanId;
            uint8_t start_index;
            if (index > 3) {
                senderCanId = 0x1FF;
                start_index = 4;
            } else {
                senderCanId = 0x200;
                start_index = 0;
            }
            float ang_result;

            // TODO: extract this
            status_lock.try_lock();
            auto &[last_ecd, ecd, angle_single_round, angular, real_current, temperature, total_angle, total_round,
                output_current] = statuses[index];
            //TODO: filter like BasicFramework
            tie(ecd, real_current, temperature) = tie(
                msg.ecd, msg.current, msg.temperature);
            angle_single_round = ECD_TO_ANGLE_DJI * static_cast<float>(ecd);
            angular = RPM_2_ANGLE_PER_SEC * static_cast<float>(msg.rpm);
            // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°
            if (ecd - last_ecd > 4096)
                total_round--;
            else if (ecd - last_ecd < -4096)
                total_round++;
            total_angle = static_cast<float>(total_round) * 360 + angle_single_round;
            last_ecd = ecd;

            pid_lock.try_lock();
            if (pos_ref[index].has_value()) {
                //指定使用位置环
                const float pos_loop_result = pos_pids[index].calculate(
                    total_angle, pos_ref[index].value(), pt);
                (void) logger.info("Position PID loop, result angular: {}", pos_loop_result);
                ang_result = ang_pids[index].calculate<Plotting>(angular, pos_loop_result, pt);
            } else {
                ang_result = ang_pids[index].calculate<Plotting>(angular, vel_ref[index], pt);
            }
            pid_lock.unlock();
            (void) logger.info("Angular PID loop, result current: {}", ang_result);
            output_current = static_cast<int16_t>(ang_result);

            array<int16_t, 4> temp{};
            for (uint8_t i = 0; i < 4; ++i) {
                temp[i] = statuses[start_index + i].output_current;
            }
            status_lock.unlock();

            can_driver->canSendMessage(static_cast<can_frame>(M3508RawControl{move(temp), senderCanId}));
        });
    }

    void M3508s::stop() const {
        can_driver->registerReadingCallback([&](can_frame &&frame) {
            (void) transformStatus(frame);
        });
    }

    void M3508s::posVelControl(array<optional<float>, 8> &&pos, array<float, 8> &&vel,
                               optional<array<PIDInstance, 8> > &&pos_pid,
                               optional<array<PIDInstance, 8> > &&vel_pid) {
        lock_guard lock(pid_lock);
        if (pos_pid.has_value()) {
            std::ranges::move(pos_pid.value(), pos_pids.begin());
        }
        if (vel_pid.has_value()) {
            std::ranges::move(vel_pid.value(), ang_pids.begin());
        }

        std::ranges::move(pos, pos_ref.begin());
        std::ranges::move(vel, vel_ref.begin());

    }

    void M3508s::velControl(array<float, 8> &&vel, optional<array<PIDInstance, 8> > &&vel_pid) {
        lock_guard lock(pid_lock);
        if (vel_pid.has_value()) {
            std::ranges::move(vel_pid.value(), ang_pids.begin());
        }
        std::ranges::fill(pos_ref, nullopt);
        std::ranges::move(vel, vel_ref.begin());
    }

    template<bool Loop, bool Func>
    void M3508s::PIDCalibration(const uint8_t motorId) {
        const TimePoint calibrationStart = steady_clock::now();

        (void) logger.info("Ready to calibrate PID, generating function.");

        constexpr auto func = [](const float t)-> float {
            if constexpr (Func == SineFunc) {
                constexpr float A = 1000.0f * std::numbers::pi;
                return A * std::sin(t);
            } else {
                return t <= 2500 ? 0 : 10000;
            }
        };

        if constexpr (Func == SineFunc) {
            (void) logger.info("Sine wave function: sin(500hz * 2*pi * t/1000ms) ");
        } else {
            (void) logger.info("Step function: 0 -> 2000mA ");
        }

        const auto index = motorId - 1;
        const uint16_t senderCanId = index > 3 ? 0x1FF : 0x200;
        vector<pair<float, float> > data;
        can_driver->registerReadingCallback([&](can_frame &&frame) {
            TimePoint pt = steady_clock::now();
            const auto msg = static_cast<M3508RawStatus>(frame);
            const auto real_current = msg.current;
            const auto angular = RPM_2_ANGLE_PER_SEC * static_cast<float>(msg.rpm);
            (void) logger.info("Receiving Status:\n {}", msg.format());
            fuse_check(msg);

            const float func_result = func(std::chrono::duration<float, std::milli>(pt - calibrationStart).count());

            float pid_result;
            array<int16_t, 4> temp{};
            if constexpr (Loop == AngularLoop) {
                pid_result = ang_pids[motorId].calculate<EnablePlotting>(
                    angular, func_result, pt);
                (void) logger.info("Angular PID loop, ref: {}, output: {}", func_result, pid_result);
                data.emplace_back(angular, pid_result);
                temp[index] = static_cast<int16_t>(pid_result);
            } else {
                // pid_result = cur_pids[motorId].calculate<EnablePlotting>(
                //     real_current, func_result, pt);
                // (void) logger.info("Current PID loop, ref: {}, output: {}", func_result, pid_result);
                (void) logger.debug("measure: {}, Func: {}", real_current, func_result);
                data.emplace_back(real_current, func_result);
                temp[index] = static_cast<int16_t>(func_result);
            }
            can_driver->canSendMessage(static_cast<can_frame>(M3508RawControl{move(temp), senderCanId}));
        });
        (void) logger.info("Calibration have started. Wait for 5 seconds...");
        std::this_thread::sleep_for(std::chrono::seconds(5));
        stop();
        try { export_CSV(data); } catch (const std::exception &e) {
            (void) logger.fatal("Export CSV Error: {}", e.what());
        };
    }

    void M3508s::export_CSV(vector<pair<float, float> > &data) const {
        auto dir = string(MM_CSV_DIR);
        const string path = vformat("{}/M3508", format_args(make_format_args(dir)));
        (void) logger.info("Exporting CSV to: {}", path);
        std::filesystem::create_directories(path);
        std::ofstream input_ofstream(path + "/SystemInput.csv"), output_ofstream(path + "/SystemOutput.csv");

        if (!input_ofstream.is_open() || !output_ofstream.is_open()) {
            throw std::runtime_error("Failed to open csv output files.");
        }

        for (const auto &[measure, pid_out]: data) {
            input_ofstream << pid_out << '\n';
            output_ofstream << measure << '\n';
        }

        output_ofstream.close();
        input_ofstream.close();
    }

    void M3508s::fuse_check(const M3508RawStatus &status) {
        if (status.temperature >= MM_FUSE_TEMPERATURE) {
            throw std::runtime_error(vformat(
                "M3508 ID: 0x{:X} has exceeded fuse temperature at: {} degree, please check the hardware",
                make_format_args(status.canId, status.temperature)));
        }
    }

    template void M3508s::enable<true>();

    template void M3508s::enable<false>();

    template void M3508s::PIDCalibration<true, true>(const uint8_t motorId);

    template void M3508s::PIDCalibration<true, false>(const uint8_t motorId);

    template void M3508s::PIDCalibration<false, true>(const uint8_t motorId);

    template void M3508s::PIDCalibration<false, false>(const uint8_t motorId);
}
